import java.util.ArrayList;

public class GJKHelper
{
	private final int LOOP_MAX = 1000000;

	public boolean isIntersecting(Mesh m1, Mesh m2)
	{
		Vector firstAverage = getAverageDir(m1);
		Vector secondAverage = getAverageDir(m2);
		Vector guessDirection = difference(secondAverage, firstAverage);
		Vector firstPoint = minkowskiSupport(guessDirection, m1, m2);
		Vector curDirection = negativeVector(firstPoint);

		ArrayList<Vector> simplexList = new ArrayList<Vector>();
		simplexList.add(firstPoint);

		int loopNum = 0;
		while(loopNum < LOOP_MAX)
		{
			loopNum++;
			System.out.println(loopNum);
			Vector curGuess = minkowskiSupport(curDirection, m1, m2);
			if(!isFacing(curGuess, curDirection)) 
			{
				System.out.println("Collision returned no intersection");
				return false;
			}
			simplexList.add(curGuess);
			simplexList = simplexTest(simplexList, curDirection);
			if(simplexList.isEmpty()) return true;
			curDirection = simplexList.get(simplexList.size() - 1);
			simplexList.remove(simplexList.size() - 1);
		}
		System.err.println("Collision Failed!");
		return false;
	}

	public double calculateDotProduct(Vector v1, Vector v2)
	{
		return (v1.getX() * v2.getX()) + (v1.getY() * v2.getY()) + (v1.getZ() * v2.getZ());
	}

	public boolean isFacing(Vector v1, Vector v2)
	{
		if(calculateDotProduct(v1, v2) > 0) return true;
		return false;
	}

	public Vector calculateCrossProduct(Vector v1, Vector v2)
	{
		double a = determinant(v1.getY(), v1.getZ(), v2.getY(), v2.getZ());
		double b = determinant(v1.getX(), v1.getZ(), v2.getX(), v2.getZ());
		double c = determinant(v1.getX(), v1.getY(), v2.getX(), v2.getY());

		Vector v = new Vector(a, -b, c);
		return v;
	}

	public double determinant(double topL, double topR, double botL, double botR)
	{
		return topL * botR - topR * botL;
	}

	public Vector getAverageDir(Mesh m)
	{
		double averageX = 0, averageY = 0, averageZ = 0;
		int num = 0;
		for(double[] d : m.getUnorderedVertices())
		{
			averageX += d[0];
			averageY += d[1];
			averageZ += d[2];
			num++;
		}
		averageX /= num;
		averageY /= num;
		averageZ /= num;
		Vector v = new Vector(averageX, averageY, averageZ);
		return v;
	}

	//Hill Climbing!
	public int supportFunction(Vector dir, Mesh m)
	{
		ArrayList<double[]> vertices = m.getUnorderedVertices();
		int id = 0;
		Vector newVec = new Vector(vertices.get(id));
		double length = calculateDotProduct(newVec, dir);
		double lastLength = Double.NEGATIVE_INFINITY;

		System.out.println("DOING THE FLIPPING ALGORITHM YOU ABSOLUTE DISASTER OF A HUMAN BEING");
		while(length > lastLength)
		{
			ArrayList<Integer> neighbors = m.getNeighbors(id);
			lastLength = length;
			System.out.println(length + " is the current length");
			for(Integer i : neighbors)
			{
				newVec = new Vector(vertices.get(i));
				if(calculateDotProduct(newVec, dir) > length) length = calculateDotProduct(newVec, dir);
				System.out.println("All of the neighbors:" + length);
				id = i;
			}
		}

		return id;
	}

	public Vector minkowskiSupport(Vector dir, Mesh m1, Mesh m2)
	{
		System.out.println("THIS IS NOT EXPLICIT MINKOWSKI YOU FAILURE");
		Vector firstPoint = new Vector(m1.getUnorderedVertices().get(supportFunction(dir, m1)));
		Vector oppDir = negativeVector(dir);
		Vector secondPoint = new Vector(m2.getUnorderedVertices().get(supportFunction(dir, m2)));
		Vector minkowski = difference(firstPoint, secondPoint);
		return minkowski;
	}

	public Vector difference(Vector v1, Vector v2)
	{
		Vector difference = new Vector(v1.getX() - v2.getX(), v1.getY() - v2.getY(), v1.getZ() - v2.getZ());
		return difference;
	}

	public Vector negativeVector(Vector v)
	{
		return new Vector(-v.getX(), -v.getY(), -v.getZ());
	}

	public ArrayList<Vector> simplexTest(ArrayList<Vector> simplex, Vector dir)
	{
		if(simplex.size() == 2)
		{
			Vector ab = difference(simplex.get(0), simplex.get(1));
			Vector ao = negativeVector(simplex.get(1));
			if(isFacing(ab, dir)) 
			{
				simplex.add(calculateCrossProduct(calculateCrossProduct(ab, ao), ab));
			}
			else
			{
				simplex.remove(0);
				simplex.add(ao);
			}
		}
		else if(simplex.size() == 3)
		{
			Vector ab = difference(simplex.get(1), simplex.get(2));
			Vector ac = difference(simplex.get(0), simplex.get(2));
			Vector abc = calculateCrossProduct(ab, ac);
			Vector ao = negativeVector(simplex.get(2));
			if(isFacing(calculateCrossProduct(abc, ac), dir))
			{
				if(isFacing(ac, dir))
				{
					simplex.remove(1);
					simplex.add(calculateCrossProduct(calculateCrossProduct(ac, ao), ac));
				}
				else
				{
					simplex.remove(0);
					simplex.remove(0);
					simplex.add(ao);
				}
			}
			else
			{
				if(isFacing(ab, dir))
				{
					simplex.remove(0);
					simplex.add(calculateCrossProduct(calculateCrossProduct(ab, ao), ab));
				}
				else if(isFacing(negativeVector(ac), ao))
				{
					simplex.remove(0);
					simplex.remove(0);
					simplex.add(ao);
				}
				else
				{
					if(isFacing(abc, ao))
					{
						simplex.add(abc);
					}
					else
					{
						simplex.add(negativeVector(abc));
					}
				}
			}
		}
		else
		{
			Vector ao = negativeVector(simplex.get(3));

			Vector ab = difference(simplex.get(3), simplex.get(2));
			Vector ac = difference(simplex.get(3), simplex.get(1));
			Vector ad = difference(simplex.get(3), simplex.get(0));
			Vector bc = difference(simplex.get(2), simplex.get(1));
			Vector bd = difference(simplex.get(2), simplex.get(0));
			Vector cd = difference(simplex.get(1), simplex.get(0));

			Vector abc = calculateCrossProduct(ab, ac);
			Vector acb = calculateCrossProduct(ac, ab);
			Vector abd = calculateCrossProduct(ab, ad);
			Vector adb = calculateCrossProduct(ad, ab);
			Vector acd = calculateCrossProduct(ac, ad);
			Vector adc = calculateCrossProduct(ad, ac);

			boolean facingABC = isFacing(abc, ao);
			boolean facingACB = isFacing(acb, ao);
			boolean facingABD = isFacing(abd, ao);
			boolean facingADB = isFacing(adb, ao);
			boolean facingACD = isFacing(acd, ao);
			boolean facingADC = isFacing(adc, ao);

			boolean facingAbcSides = facingABC || facingACB;
			boolean facingAbdSides = facingABD || facingADB;
			boolean facingAcdSides = facingACD || facingADC;

			if(facingAbcSides && facingAbdSides && facingAcdSides)
			{
				simplex = new ArrayList<Vector>();
			}
			else
			{
				if(!facingAbcSides)
				{
					if(!facingAbdSides)
					{
						simplex.remove(2);
						simplex = simplexTest(simplex, dir);
					}
					else
					{
						simplex.remove(1);
						simplex = simplexTest(simplex, dir);
					}
				}
				else if(!facingAbdSides)
				{
					simplex.remove(0);
					simplex = simplexTest(simplex, dir);
				}
			}
		}
		return simplex;
	}
}